#include "Quad4SmallDrillMem.h"
#include "SGFem/Integration/IntegrationFormula/Gauss2D1X1P.h"

using SG::DataStructure::Common::Real;
using SG::Algebra::Matrix;
using SG::DataStructure::FEM::ShellSecMaterialConstant;
using SG::FEMSolver::Element::Gauss2D1X1P;
using SG::FEMSolver::Element::Gauss2D2X2P;
std::size_t Quad4SmallDrillMem::s_nNode = 4;
// std::size_t Quad4SmallDrillMem::m_nNode = 1;
void Quad4SmallDrillMem::Initialize ()
{
    const auto& integ0 = m_integForm0.GetPoints ();
    m_shpFunc0.Compute (integ0);
    
    const auto& integ = m_integForm.GetPoints ();
    m_shpFunc.Compute (integ);

    m_incomShpFunc.Compute (integ);
}

/// @brief 单元普通应变部分
void Quad4SmallDrillMem::ComputeStrainMatrix(std::size_t iGauss, const Matrix& coord, _OUT Matrix& B, _OUT Real& j, bool isCenter) const
{
    Matrix DNDxi;
    if (isCenter)
    {
        DNDxi = m_shpFunc0.GetShapeFunctionDerivative(0);
    }
    else
    {
        DNDxi = m_shpFunc.GetShapeFunctionDerivative(iGauss);
    }
    auto JT = DNDxi * coord;
    j = JT(0, 0) * JT(1, 1) - JT(0, 1) * JT(1, 0);
    auto JTinv = SG::Algebra::inverse2X2 (JT);
    auto DNDx = JTinv * DNDxi;

    B = Matrix(3, 12);
    for (std::size_t iNode = 0; iNode < s_nNode; ++iNode)
    {
        B(0, iNode*3)     = DNDx(0, iNode);
        B(1, iNode*3 + 1) = DNDx(1, iNode);
        B(2, iNode*3)     = DNDx(1, iNode);
        B(2, iNode*3 + 1) = DNDx(0, iNode);
    }
}


// @brief 使用求逆犯规法计算积分点Jacobian
void Quad4SmallDrillMem::ComputeIncomStrainMatrix (std::size_t iGauss, SG::DataStructure::Common::Real j, const Matrix& JT0, _OUT Matrix& G) const
{
    // 伴随矩阵的转置
    Matrix JT0Star (2, 2); 
    JT0Star (0, 0) = JT0(1, 1);
    JT0Star (1, 1) = JT0(0, 0);
    JT0Star (0, 1) = -JT0(0, 1);
    JT0Star (1, 0) = -JT0(1, 0);

    Matrix DMDx = JT0Star * (1.0/j) * m_incomShpFunc.GetShapeFunctionDerivative(iGauss);
    G = Matrix(3, 4);
    G(0, 0) = DMDx(0, 0);
    G(1, 1) = DMDx(1, 0);
    G(2, 0) = DMDx(1, 0);
    G(2, 1) = DMDx(0, 0);
    G(0, 2) = DMDx(0, 1);
    G(1, 3) = DMDx(1, 1);
    G(2, 2) = DMDx(1, 1);
    G(2, 3) = DMDx(0, 1);
}

/** @brief 计算单元积分点应变
 * 
 * 需要考虑非协调应变对应应变的贡献
 */
void Quad4SmallDrillMem::ComputeIntegStrain (const Matrix& localDisp, const Matrix& localCoord, 
    const Matrix& Cm0,
    const SG::DataStructure::Common::Real* const thickness,
    Matrix& _OUT integStrain)
{

    Matrix kud (12, 4);
    Matrix kdd (4, 4);
    auto        JT0 = m_shpFunc0.GetShapeFunctionDerivative (0) * localCoord;
    //const auto& Cm  = secConstant.m_Cm;

    Matrix     B;
    Real       j;
    Matrix Cm;
    const auto nGauss = m_integForm.GetPointSize ();
    m_incomShpFunc.Compute (m_integForm.GetPoints ());
    
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        ComputeStrainMatrix (iGauss, localCoord, B, j);
        // --- Incompatible part
        Matrix G (3, 4);
        ComputeIncomStrainMatrix (iGauss, j, JT0, G);       
        Cm = Cm0 *thickness[iGauss];

        const Real tmp = j * m_integForm.GetPoint (iGauss).m_weight;
        auto       BT  = B.Transpose ();
        kud += BT * Cm * G * tmp;
        kdd += G.Transpose () * Cm * G * tmp;
    }
    Matrix inv_kdd = kdd.Inverse ();
    Matrix tmp = inv_kdd * (kud.Transpose ());

    // 单元非协调位移
    auto innerEleDisplacement = tmp*localDisp*-1.0;
    
    // 计算膜积分点应变
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        Matrix Bm;
        Real jm;
        ComputeStrainMatrix (iGauss, localCoord, Bm, jm);
        // 组装计算Membrane单元非协调部分应变矩阵G
        Matrix      G (3, 4);
        const auto& DNDxi = m_shpFunc.GetShapeFunctionDerivative (iGauss);
        auto        JT    = DNDxi * localCoord;
        Matrix      DMDx  = JT.Inverse () * m_incomShpFunc.GetShapeFunctionDerivative (iGauss);
        G (0, 0)          = DMDx (0, 0);
        G (1, 1)          = DMDx (1, 0);
        G (2, 0)          = DMDx (1, 0);
        G (2, 1)          = DMDx (0, 0);
        G (0, 2)          = DMDx (0, 1);
        G (1, 3)          = DMDx (1, 1);
        G (2, 2)          = DMDx (1, 1);
        G (2, 3)          = DMDx (0, 1);

        // 计算膜部分应变：协调部分+非协调部分
        auto iIntegStrain = Bm*localDisp + G*innerEleDisplacement;
        
        //组装到总的积分点应变矩阵
        integStrain[0][iGauss] = iIntegStrain[0][0];
        integStrain[1][iGauss] = iIntegStrain[1][0];
        integStrain[3][iGauss] = iIntegStrain[2][0];
    }
}

/** @brief 计算单元中心点应变
 * 
 * 不需要考虑非协调应变对应应变的贡献
 */
void Quad4SmallDrillMem::ComputeIntegStrain0 (const Matrix& localDisp, const Matrix& localCoord, 
    // const ShellSecMaterialConstant& secConstant,
    // const SG::DataStructure::Common::Real* const thickness,
    Matrix& _OUT integStrain)
{
    Matrix Bm;
    Real jm;
    ComputeStrainMatrix (0, localCoord, Bm, jm, true);
    // 计算膜部分应变：协调部分
    auto iIntegStrain = Bm * localDisp;
    
    //组装到总的中心点应变矩阵
    integStrain[0][0] = iIntegStrain[0][0];
    integStrain[1][0] = iIntegStrain[1][0];
    integStrain[3][0] = iIntegStrain[2][0];
}

Matrix Quad4SmallDrillMem::ComputeStiffness 
(
const Matrix& coord, 
const SG::DataStructure::Common::Real* const thickness,
const ShellSecMaterialConstant& SecConstant,
const Matrix& T1,
const bool& transFlag
) const
{
    Matrix ke (12, 12);
    Real GV{0.0};
    Matrix kud (12, 4);
    Matrix kdd (4, 4);

    auto JT0 = m_shpFunc0.GetShapeFunctionDerivative(0) * coord;

    Matrix B, G;
    Real j;
    const auto nGauss = m_integForm.GetPointSize();
    for (std::size_t iGauss = 0; iGauss < nGauss; ++iGauss)
    {
        ComputeStrainMatrix (iGauss, coord, B, j);
        // Incompatible part
        ComputeIncomStrainMatrix (iGauss, j, JT0, _OUT G);
        // --- Incompatible part
        const Real tmp = j * m_integForm.GetPoint(iGauss).m_weight;
        auto BT = B.Transpose();

        Matrix Cm;
        SecConstant.GetCm (thickness[iGauss], _OUT Cm); // 包含`厚度`
        //若需要材料坐标转换
        if (transFlag)
          Cm = T1 * Cm * T1.Transpose ();
           
        // ToDo 变厚度情况重新计算 Cm 矩阵
        ke  += BT * Cm * B * tmp;
        kud += BT * Cm * G * tmp;
        kdd += G.Transpose() * Cm * G * tmp;
        GV += tmp * Cm(2, 2);
    }

    // condence
    ke -= (kud*kdd.Inverse()*kud.Transpose());

    // drill
    const Real KROT6 =100.0;

    Real delta2{1.0e-4 * KROT6};
    auto DN0Dx = JT0.Inverse() * m_shpFunc0.GetShapeFunctionDerivative(0);

    Matrix Q (1, 12, {-0.5*DN0Dx (1, 0), 0.5*DN0Dx (0, 0), -0.25,
                      -0.5*DN0Dx (1, 1), 0.5*DN0Dx (0, 1), -0.25,
                      -0.5*DN0Dx (1, 2), 0.5*DN0Dx (0, 2), -0.25,
                      -0.5*DN0Dx (1, 3), 0.5*DN0Dx (0, 3), -0.25,});
    auto tmp = GV * delta2;
    auto tmpp=Q.Transpose() * (Q * tmp);
    ke += Q.Transpose() * (Q * tmp);
    
    Matrix Q2 (1, 12, {0, 0, 0.25,
                      0, 0, -0.25,
                      0, 0, 0.25,
                      0, 0, -0.25});

    auto lambda = 1.0e-10 * GV* KROT6/4.0;
    auto tmpppp=Q2.Transpose() * (Q2 * lambda);
    ke += Q2.Transpose() * (Q2 * lambda);

    return ke;
}
